#include "trapTraj.h"

#define trap_abs(x)	(x > 0 ? x : -x)
#include "stdio.h"

/**
 * @brief  初始化参数
 * @param  void
 * @retval void
 */
void TRAJ_plan(Traj_Typedef *Traj,float dX, float velBase, float velMax, float accXin, float decXin)
{
		/* 极性判断 */
		if (dX < 0) {
			accXin = -trap_abs(accXin);
			decXin = -trap_abs(decXin);
			velMax = -trap_abs(velMax);
		}
		
		/* 设置的加速过程加加速度和减速阶段的加加速度 */
    Traj->temp.velMax =  velMax;  // 最大速度
		Traj->temp.velBase = velBase; // 
	
		/* 最小加减速位移 */
    float dXmin = accXin + decXin;
		float dXspeed = 0.0f;
	
    // Are we displacing enough to reach cruising speed?
		// 用在加减速的位移真的会小于总位移吗？
    if (dX < dXmin) {
		// 重新计算加速距离和减速距离
		float dx = accXin + decXin;
		accXin = accXin / dx * dX;
		decXin = decXin / dx * dX;
        Traj->temp.speedTime = 0.0f;
    } else {
			// Long move (trapezoidal profile)
			// 求出剩余匀速时间
			Traj->temp.speedTime = (dX - dXmin) / velMax;
			dXspeed = dX - dXmin;
    }
		
		float accXin_2 = accXin / 6.0f;
		float decXin_2 = decXin / 6.0f;
		
		// 加速和减速所需时间
		/* 不要问我velBase为什么要加上，我试了一个多小时 */
		float accelTime_2 = trap_abs((accXin_2 * 3) / ((velMax + velBase) / 2.0f));
		float decelTime_2 = trap_abs((decXin_2 * 3) / (velMax / 2.0f));
		
		Traj->temp.accelTime = accelTime_2 * 2;
		Traj->temp.decelTime = decelTime_2 * 2;
				
		// 计算实际加加速度 
		Traj->temp.accj = (velMax-velBase) / (accelTime_2 * accelTime_2);
		
		Traj->temp.decj = velMax / (decelTime_2 * decelTime_2);

		// 计算各个时刻的初加速度
		Traj->temp.a1 = Traj->temp.accj * accelTime_2;
		Traj->temp.a2 = Traj->temp.a1 + (-Traj->temp.accj) * accelTime_2;
		Traj->temp.a3 = -Traj->temp.decj * decelTime_2;	
		
		// 计算各个时刻的初速度
		Traj->temp.v1 = velBase + ((1/2.f) * Traj->temp.accj * powf(accelTime_2, 2));
		Traj->temp.v2 = Traj->temp.v1 + Traj->temp.a1 * accelTime_2 + ((1/2.f) * (-Traj->temp.accj) * powf(accelTime_2, 2));
		Traj->temp.v3 = Traj->temp.v2 + Traj->temp.a2 * accelTime_2 + ((1/2.f) * (-Traj->temp.decj) * powf(decelTime_2, 2));
		
		
		// 计算各个时刻的位置
		Traj->temp.s1 = velBase * accelTime_2 + ((1/6.f) * Traj->temp.accj * powf(accelTime_2, 3));
		Traj->temp.s2 = Traj->temp.s1 + Traj->temp.v1*accelTime_2 + ((1/2.f) * Traj->temp.a1 * powf(accelTime_2, 2)) + ((1/6.f) * -Traj->temp.accj * powf(accelTime_2, 3));
		Traj->temp.s3 = accXin + dXspeed;
		Traj->temp.s4 = Traj->temp.s3 + Traj->temp.v2*decelTime_2 + ((1/2.f) * Traj->temp.a2 * powf(decelTime_2, 2)) + ((1/6.f) * -Traj->temp.decj * powf(decelTime_2, 3));

    // Fill in the rest of the values used at evaluation-time
    Traj->TotalTime = Traj->temp.accelTime + Traj->temp.speedTime + Traj->temp.decelTime;
		
    Traj->temp.EndPoint = dX;
		
		Traj->t = 0;
		Traj->step = 0;
}

/**
 * @brief  计算各个阶段的值
 * @param  void
 * @retval void
 */
void TRAJ_eval(Traj_Typedef *Traj, float dt)
{
		Traj->t += dt;	/* 以假定速度来运行，因为无法预测下一速度时刻的 */
	
		if (Traj->t > 32700) {
			Traj->t = 32700;
		}
	
		float t = Traj->t;	/* 重新构建时间t，方便阅读后续源码 */
	
    if (t < 0.0f) {  // Initial Condition
				Traj->step = 0;
			
        Traj->s  = 0.0f;
        Traj->v  = 0.0f;
        Traj->a  = 0.0f;
				Traj->j  = 0.0f;
    } else if (t < Traj->temp.accelTime/2.0f) {  // 加速度加速阶段，就是以恒定的j来计算
			/*
				以公式构建模型
				x = (1/6) * j * t*t*t
				v = (1/2) * j * t2
				a = j * t
				j = 设置的j
			*/
				Traj->step = 1;
			
				Traj->j  = Traj->temp.accj;
				Traj->a  = Traj->temp.accj * t;
				Traj->v  = Traj->temp.velBase + (1/2.f) * Traj->temp.accj * powf(t, 2);
				Traj->s  = Traj->temp.velBase * t + (1/6.f) * Traj->temp.accj * powf(t, 3);
			
    } else if (t < Traj->temp.accelTime) {  // 加速度减速阶段，就是以恒定的-j来计算
			/*
			以公式构建模型,与上式的区别就在于多了初始位置的状态
				x = x0 + v0 * t + (1/2) * a0 * t2 + (1/6) * j * t*t*t
				v = v0 + a0 * t + (1/2) * j * t2
				a = a0 + j * t
				j = 设置的j
			*/
				Traj->step = 2;
			
				t = t - (Traj->temp.accelTime/2.0f);
			
				Traj->j  = -Traj->temp.accj;
				Traj->a  = Traj->temp.a1 + (-Traj->temp.accj) * t;
				Traj->v  = Traj->temp.v1 + Traj->temp.a1 * t + (1/2.f) * (-Traj->temp.accj) * powf(t, 2);
				Traj->s  = Traj->temp.s1 + (Traj->temp.v1 * t) + ((1/2.f) * Traj->temp.a1 * powf(t, 2)) + ((1/6.f) * (-Traj->temp.accj) * powf(t, 3));
			
		} else if (t < Traj->temp.accelTime + Traj->temp.speedTime) {  // Coasting
				/*	
					匀速行驶
				*/
				Traj->step = 3;
			
				t = t - Traj->temp.accelTime;
				Traj->j  = 0.0f;
        Traj->a  = 0.0f;
				Traj->v  = Traj->temp.velMax;
				Traj->s  = Traj->temp.s2 + Traj->v * t;
			
    } else if (t < Traj->temp.accelTime + Traj->temp.speedTime + (Traj->temp.decelTime/2.0f)) {  // 减速的加速阶段
				/*
					匀速阶段的开始和结束加速度和速度一样
				*/
				Traj->step = 4;
			
        t = (t - (Traj->temp.accelTime + Traj->temp.speedTime));
			
				Traj->j  = -Traj->temp.decj;
				Traj->a  = -Traj->temp.decj * t;
				Traj->v  =  Traj->temp.velMax + (1/2.f) * (-Traj->temp.decj) * powf(t, 2);
				Traj->s  =  Traj->temp.s3 + (Traj->temp.velMax * t) + ((1/6.f) * (-Traj->temp.decj) * powf(t, 3));
    } else if (t < Traj->TotalTime) {
				Traj->step = 5;
			
				t = (t - (Traj->temp.accelTime + Traj->temp.speedTime + (Traj->temp.decelTime/2.0f)));
				Traj->j  = Traj->temp.decj;
				Traj->a  = Traj->temp.a3 + (Traj->temp.decj) * t;
				Traj->v  = Traj->temp.v3 + Traj->temp.a3 * t + (1/2.f) * (Traj->temp.decj) * powf(t, 2);
				Traj->s  = Traj->temp.s4 + (Traj->temp.v3 * t) + ((1/2.f) * Traj->temp.a3 * powf(t, 2)) + ((1/6.f) * (Traj->temp.decj) * powf(t, 3));
		}else if (t >= Traj->TotalTime) {  // Final Condition
				Traj->step = 6;
			
        Traj->s	 = Traj->temp.EndPoint;
        Traj->v  = 0.0f;
        Traj->a  = 0.0f;
				Traj->j  = 0.0f;
    } else {
				Traj->step = 7;
        // TODO: report error here
    }
}

/******************* (C) COPYRIGHT 2023 FUYEBING*****END OF FILE************/
